import lejos.nxt.Motor;
import lejos.nxt.Sound;
import lejos.nxt.UltrasonicSensor;

public class USPivot extends Thread {
	private UltrasonicSensor us;
	private Motor motor;
	private Bridge_M bridge;
//	private boolean stop;
	private boolean quiet;
	private int MAXDIST = 25;
	
	public USPivot(Motor motor, UltrasonicSensor us, Bridge_M bridge, int maxDist) {
		this.motor = motor;
		this.us = us;
		this.bridge = bridge;
		this.motor.resetTachoCount();
		this.motor.setSpeed(300);
		this.MAXDIST = maxDist;
//		System.out.println("USPivot initialized");
	}
	
	public void run() {
//		System.out.println("USPivot running");
		// rotate 180 degrees from left to right
		int sweep = 90;	
		// Ausgangsposition setzen
		motor.resetTachoCount();
		
		while (true) {
			int distance = 0;
			int angle = 0;
			if(!motor.isRotating()) {
				sweep = -sweep;
				motor.rotateTo(sweep, true);
			}
			while (motor.isRotating()) { // Werte anpassen
				Thread.yield();
				int avgdist = 0;
				for (int i = 0; i < 10; i++) {
					distance = us.getDistance();
					angle = motor.getTachoCount();
					avgdist += distance;
				}
				avgdist = avgdist / 11;
				System.out.println("Distance" + new Integer(avgdist).toString());
				System.out.println("Angle" + new Integer(angle).toString());
				if (Math.abs(angle) > 10) {
//					if (distance >= 30 && !quiet) {
					if (avgdist >= MAXDIST) {
						bridge.setDistanceAngle(distance, angle);
//						quiet = true;
					}
				}
			}
		}
	}
	
//	public void continueSignaling() {
//		quiet = false;
//	}
	
	private void testValue(int distance, int angle) {
//		if (distance > 10) Sound.beep();
		if (distance > 20) Sound.twoBeeps();
		else if (distance > 30) Sound.buzz();
	}
}
